
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Import/RobotImporterFactory.h>

#include <Inventor/Qt/SoQt.h>
#include <QFileInfo>

#include <string>
#include <map>
#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>

using std::cout;
using std::endl;
using namespace VirtualRobot;

int main(int argc, char *argv[])
{
    // initilizing Coin and SoQt, otherwise the ColladaConverter will not work
    SoDB::init();
    SoQt::init(argc,argv,"dae2mmm converter");

    cout << " --- Collada to MMM converter --- " << endl;

    // we are expecting a single argument, and that is the filename of the file to convert
    if (argc<2)
    {
        cout << "Missing filename as first argument, aborting..." << endl;
        cout << "Syntax: dae2mmm %filename" << endl;
        //MMM_ERROR << endl << "Could not process command line, aborting..." << endl;
        return -1;
    }
    //c.print();


    std::string filename = std::string(argv[1]);
    cout << "Filename provided for conversion: " << filename << endl;

    if (!filename.empty())
    {
        QFileInfo fileInfo(filename.c_str());
        std::string suffix(fileInfo.suffix().toLatin1());
        std::string path(fileInfo.path().toLatin1());
        std::string file(fileInfo.fileName().toLatin1());
        file = file.substr(0, file.size()-4);
        std::string newFile = file + ".xml";
        std::string newFilename = path + "/" + newFile;

        std::transform(suffix.begin(), suffix.end(), suffix.begin(), ::tolower);
        cout << "Input File is [" << filename << "]" << endl;
#ifdef false
        cout << "Path to File is [" << path << "]" << endl;
        cout << "Filename is [" << file << "]" << endl;
        cout << "Suffix is [" << suffix << "]" << endl;
        cout << "New Filename will be [" << newFile << "]" << endl;
#endif
        cout << "MMM-Model will be saved as [" << newFilename << "]" << endl;

        // For now, we just convert Collada Files
        if (suffix.compare("dae")!=0)
        {
            cout << "Expected Collada File as input (.dae). Aborting..." << endl;
            return -1;
        }
        VirtualRobot::RobotPtr robotTemp;
        try
        {
            cout << "Setting up Converter..." << endl;
            RobotImporterFactoryPtr importer = RobotImporterFactory::fromFileExtension(suffix,nullptr);
            if (!importer)
            {
                cout << " ERROR while grabbing importer" << endl;
                return -1;
            }
            else
                cout << "Converter initialized..." << endl;
            robotTemp = importer->loadFromFile(filename, RobotIO::eFull);

            cout << "Saving file..." << endl;

            RobotIO::saveXML(robotTemp, newFile, path);

            cout << "Finished." << endl;

        }
        catch (VirtualRobotException &e)
        {
            cout << " ERROR while creating robot" << endl;
            cout << e.what();
            return -1;
        }
    }
    return 0;
}
